#include "CleanAlgorithm.h"

#include <math.h>
#include <omp.h>

#include <stdlib.h>
#include <stdio.h>

#define HYPOT(x, y) sqrt((x) * (x) + (y) * (y))

void Analyze(int SpectrumSize, int SampleCount, double *CosArray, double *SinArray, double *SpectrumRe, double *SpectrumIm, double *SpectrumAbs, double *SamplesRe, double *SamplesIm, int NumResults, int *ResultFrequencies, double *ResultAmplitudes, double *WindowRe, double *WindowIm)
{
	int PeakFrequency;
	double PeakAmplitude;
	int i;
	
	Ndft(&PeakAmplitude, &PeakFrequency, SpectrumSize, SampleCount, CosArray, SinArray, SpectrumRe, SpectrumIm, SpectrumAbs, SamplesRe, SamplesIm);
	
	ResultFrequencies[0] = PeakFrequency;
	ResultAmplitudes[0] = PeakAmplitude;

	for (i = 1; i < NumResults; i++)
	{
		Clean(SpectrumSize, &PeakFrequency, &PeakAmplitude, SpectrumRe, SpectrumIm, SpectrumAbs, WindowRe, WindowIm, SampleCount);
		
		ResultFrequencies[i] = PeakFrequency;
		ResultAmplitudes[i] = PeakAmplitude;
	}
}

void Ndft(double *PeakAmplitude, int *PeakFrequency, int SpectrumSize, int SampleCount, double *CosArray, double *SinArray, double *SpectrumRe, double *SpectrumIm, double *SpectrumAbs, double *SamplesRe, double *SamplesIm)
{
	int i, j;
	int LocalPeakFrequency;
	double LocalPeakAmplitude;
	
#ifdef WIN32
#define SPECTRUM_TXT "spectrum_windows.txt"
#else
#define SPECTRUM_TXT "spectrum.txt"
#endif
	FILE *fp = fopen(SPECTRUM_TXT, "w");
//#pragma omp parallel private(i, j)
	{
//#pragma omp for schedule(static)
		for (i = 0; i < SpectrumSize; i++)
		{
			double SumRe = 0;
			double SumIm = 0;
			
			for (j = 0; j < SampleCount; j++)
			{
				double x = CosArray[i * SampleCount + j];
				double y = SinArray[i * SampleCount + j];
				SumRe += SamplesRe[j] * x - SamplesIm[j] * y;
				SumIm += SamplesRe[j] * y + SamplesIm[j] * x;
			}
			
			SpectrumRe[i] = SumRe;
			SpectrumIm[i] = SumIm;
			SpectrumAbs[i] = HYPOT(SumRe, SumIm);
			
			fprintf(fp, "%lf, %lf, %lf\n", SpectrumRe[i], SpectrumIm[i], SpectrumAbs[i]);
		}
	}
	
	fclose(fp);

	LocalPeakFrequency = 0;
	LocalPeakAmplitude = SpectrumAbs[0];
	
	for (i = 1; i < SpectrumSize; i++)
	{
		if (SpectrumAbs[i] > LocalPeakAmplitude)
		{
			LocalPeakAmplitude = SpectrumAbs[i];
			LocalPeakFrequency = i;
		}
	}

	*PeakFrequency = LocalPeakFrequency;
	*PeakAmplitude = LocalPeakAmplitude / SampleCount;
}


void Clean(int SpectrumSize, int *PeakFrequency, double *PeakAmplitude, double *SpectrumRe, double *SpectrumIm, double *SpectrumAbs, double *WindowRe, double *WindowIm, int SampleCount)
{
	int WindowOffset = SpectrumSize - *PeakFrequency;
	double WindowPhase = atan2(SpectrumIm[*PeakFrequency], SpectrumRe[*PeakFrequency]);
	int i;
	int LocalPeakFrequency;
	double LocalPeakAmplitude;
	
	double CosPhase = cos(WindowPhase);
	double SinPhase = sin(WindowPhase);

	// das ndft window enthaelt das spektrum, das eine bestimmte frequenz im
	// zeitbereich erzeugen wuerde, wenn man es in den frequenzbereich transformiert.
	// es wird so verschoben, dass es auf die gefundene frequenz faellt und dann
	// vom signal abgezogen.

//#pragma omp parallel
	{
//#pragma omp for schedule(static)
		for (i = 0; i < SpectrumSize; i++)
		{
			SpectrumRe[i] -= *PeakAmplitude * (WindowRe[i + WindowOffset] * CosPhase - WindowIm[i + WindowOffset] * SinPhase); 
			SpectrumIm[i] -= *PeakAmplitude * (WindowRe[i + WindowOffset] * SinPhase + WindowIm[i + WindowOffset] * CosPhase);
			SpectrumAbs[i] = HYPOT(SpectrumRe[i], SpectrumIm[i]);
		}
	}

	// find the maximum in the new spectrum
	
	LocalPeakFrequency = 0;
	LocalPeakAmplitude = SpectrumAbs[0];

	for (i = 1; i < SpectrumSize; i++)
	{
		if (SpectrumAbs[i] > LocalPeakAmplitude)
		{
			LocalPeakAmplitude = SpectrumAbs[i];
			LocalPeakFrequency = i;
		}
	}

	*PeakFrequency = LocalPeakFrequency;
	*PeakAmplitude = LocalPeakAmplitude / SampleCount;
}

double GetResultFrequency(int SpectrumSize, double MaxDetectFrequency, int FrequencyIndex)
{
	return MaxDetectFrequency / SpectrumSize * 2.0 * FrequencyIndex - MaxDetectFrequency;
}























